from dolfin import *
from numpy import sin, cos, array,zeros
from copy import deepcopy

'''Rotate zalesak disk by velocity field (-a(y-b),a(x-b)).'''

def build(problem,points):
    '''Given ordered points buid boundary mesh and the level-set function.''' 
    N = len(points)
    
    mesh = Mesh()
    editor = MeshEditor()
    editor.open(mesh, 1, 2)
    editor.init_vertices(N)
    editor.init_cells(N)

    for i, (x, y) in enumerate(points):
        editor.add_vertex(i, x, y)
    for i in range(N):
        editor.add_cell(i, i, (i+1) % (N))

    rect = RectangleMesh(problem.LL[0],problem.LL[1],problem.UR[0],problem.UR[1],50,50) 
    V = FunctionSpace(rect, 'CG', 3)
    x0 = interpolate(Expression('x[0]'), V)
    x1 = interpolate(Expression('x[1]'), V)
    a0 = x0.vector().array()
    a1 = x1.vector().array()

    # For all nodes in V find the signed distance to the disc 
    point_outside = Point(0,0)  # A point outside the disc
    Np = x0.vector().size()
    dummymesh = Mesh()
    dummyeditor = MeshEditor()
    dummyeditor.open(dummymesh, 1, 2)
    dummyeditor.init_vertices(Np + 1)
    dummyeditor.init_cells(Np)
    dummyeditor.add_vertex(0, point_outside.x(), point_outside.y())

    inside = Function(V)
    inside.vector()[:] = 1
    # Use ray casting to determine if the point is inside (-1) or outside (+1)
    # There's probably a better way to do this than to create a dummy mesh
    for i in range(Np):
        dummyeditor.add_vertex(i + 1, a0[i], a1[i])
        dummyeditor.add_cell(i, 0, i + 1)
        ei = Edge(dummymesh, i) # Edge (ray) from vertex 0 to i + 1
        ix = mesh.intersected_cells(ei) # Odd number means you're inside
        if ix.shape[0] % 2 == 1:
            inside.vector()[i] = -1
      
    phiX_ = Function(V)

    i = 0
    # Compute the signed distance
    for xx, yy in zip(a0, a1):
        p = Point(xx, yy)
        zz = mesh.distance(p)
        phiX_.vector()[i] = zz*inside.vector()[i]
        i += 1

    return mesh,phiX_

def rotate(points,a,b,t):
    '''Rotate points by velocity field (-a(y-b),a(x-b)). Time step t. Exact solution, no need for TVD! Dont't change original points.'''
    points_out = [] 
    for point in points:
        points_out.append(\
                        array([b,b])\
                        - b*array([cos(a*t)-sin(a*t),cos(a*t)+sin(a*t)])\
                        + array([cos(a*t)*point[0]-sin(a*t)*point[1],\
                                sin(a*t)*point[0]+cos(a*t)*point[1]]\
                               )\
                        )

    return points_out

def get_phi(problem,t):
    '''Get signed distance function from the disk at time t.'''
    a = problem.a
    b = problem.b  
    rotated_points = rotate(problem.points,a,b,t)
    bdry_mesh, sdf = build(problem,rotated_points)
    return sdf
